#!/usr/bin/env python3
# coding:utf-8
""" 
功能:
    实现输入w,s,a,d命令控制车辆:油门,刹车,左转,右转,挡位切换
"""

import sys
import tty
import termios
import select
from enum import IntEnum
import threading

# ROS
import rclpy
from rclpy.node import Node

# Local
from common.msg import CarControl


# import debugpy
# debugpy.listen(6688)
# debugpy.wait_for_client()
# debugpy.breakpoint()


class Gear(IntEnum):
    P = 1  # P档
    R = 2  # 倒档
    N = 3  # 空档
    D = 4  # 前进档


def Ang2Rad(angle) -> float:
    return angle * 0.01745


def Rad2Ang(rad) -> float:
    return rad * 57.2958


Angle_15 = Ang2Rad(15)
Angle_45 = Ang2Rad(45)
Compensation_Rad = 4.0


class Noblock_terminal:
    def __init__(self):
        fd = sys.stdin.fileno()
        self.old_settings = termios.tcgetattr(fd)
        tty.setraw(sys.stdin.fileno(), termios.TCSANOW)

    def __exit__(self):
        if self.old_settings:
            self.stop_no_block()

    def get_char(self):
        ch = sys.stdin.read(1)
        # sys.stdout.write(ch)
        return ch

    def select_cmd(self, timeout=0.2):
        read_list = [sys.stdin]
        cmd = "0"
        read_ret, write_ret, err_ret = select.select(read_list, [], [], timeout)
        if read_ret:
            for fd in read_ret:
                if fd == sys.stdin:
                    cmd = sys.stdin.read(1)
                else:
                    print("unknow fd")
        else:
            # print("read timeout")
            pass

        return cmd

    def stop_no_block(self):
        fd = sys.stdin.fileno()
        termios.tcsetattr(fd, termios.TCSADRAIN, self.old_settings)
        self.old_settings = None


class Sim_Action(Node):
    def __init__(self):
        self.init_ControlCMD()
        super().__init__("sim_control_car")
        self.pub_cmd = self.create_publisher(CarControl, "/vehicle_cmd", 1)

    def thread_init(self, method="planne_move"):
        # thread_inpuc_cmd = threading.Thread(target=self.choose_menu, daemon=True)
        thread_inpuc_cmd = threading.Thread(target=self.plane_move, daemon=True)
        thread_inpuc_cmd.start()

    def init_ControlCMD(self):
        data = CarControl()
        data.acc = 0.0
        data.angle = 0.0
        data.brake = 0.0
        data.gear = Gear.P
        self.control_cmd = data

    def choose_menu(self):
        help_str = """
1 车辆移动
2 车灯,清扫,垃圾倾倒,充电,加水控制,鸣笛
"""
        cmd = input(help_str)
        if cmd == "1":
            self.plane_move()
        elif cmd == "2":
            self.other_ctrol()

    def other_ctrol(self):
        "项目,产品 独有功能, 不公开"
        pass

    def publisher_cmdvel(self, speed, angle, gear, brake):
        msg = self.control_cmd
        msg.acc = speed
        msg.angle = angle
        msg.brake = brake
        msg.gear = gear

        self.pub_cmd.publish(msg)

    def plane_move(self):
        help_str = """
\033[80D     w 加速
\033[80D a 右转+      d 左转+
\033[80D     s 减速

\033[80D    i D档
\033[80D j N档      l P档:油门0,方向0
\033[80D    k R档

\033[80D 空格 刹车+油门0, c 方向回正
\033[80D q, Esc: 退出
\033[80D h 帮助
"""

        noblock_term = Noblock_terminal()
        msg = self.control_cmd
        speed = msg.acc
        angle = msg.angle
        brake = msg.brake
        gear = msg.gear

        while True:
            cmd = noblock_term.select_cmd(timeout=0.1)
            # 前后左右移动
            if cmd == "w":
                speed += 2.0
                if speed > 50.0:
                    speed = 50.0
            elif cmd == "s":
                speed -= 2.0
                if speed < 0.0:
                    speed = 0.0
            elif cmd == "a":
                angle -= Angle_15
            elif cmd == "d":
                angle += Angle_15
            # 挡位
            elif cmd == "i":
                gear = Gear.D
                speed = 0.0
            elif cmd == "k":
                gear = Gear.R
                speed = 0.0
            elif cmd == "j":
                gear = Gear.N
            elif cmd == "l":
                gear = Gear.P
                speed = 0.0
            # 刹车
            elif cmd == " ":
                speed = 0.0
                brake += 10.0
                if brake > 70.0:
                    brake = 70.0
            elif cmd == "c":
                angle = 0.0
            elif cmd == "h":
                print(help_str)
            elif cmd == "q":  # q 按键值
                break
            elif ord(cmd) == 0x1B:  # Esc 按键值
                break
            # 超时
            elif cmd == "0":
                if brake > 0.0:
                    brake -= 10.0
            else:
                print("未知指令")
                print(help_str)

            print(
                f"\033[80D speed:{speed:.2f} angle:{angle:.2f} gear:{gear} brake:{brake:.2f}"
            )

            self.publisher_cmdvel(speed, angle, gear, brake)

        print("exit 键盘控制")
        noblock_term.stop_no_block()
        sys.exit(0)


def main(args=None):
    rclpy.init(args=args)
    sim = Sim_Action()
    sim.thread_init()

    rclpy.spin(sim)
    sim.destory_node()
    rclpy.shutdown()
    sim.move_control()


if __name__ == "__main__":
    main()
